from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    package_share_path = get_package_share_path('robot_urdf')
    default_model_path = package_share_path / 'urdf/gazebo.urdf'
    default_rviz_config_path = package_share_path / 'config/gazebo.rviz'
    gazebo_world_path = package_share_path / 'world/map.world'

    return LaunchDescription([
        DeclareLaunchArgument(name='model', default_value=str(default_model_path),
                              description='机器人 URDF 模型文件路径'),
        DeclareLaunchArgument(name='world', default_value=str(gazebo_world_path),
                              description='世界文件路径'),
        DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
                              description='RVIZ 配置文件路径'),
        DeclareLaunchArgument(name='robotname', default_value='whitebot',
                              description='机器人名字'),

        ExecuteProcess(
            cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
                 '-s', 'libgazebo_ros_factory.so', LaunchConfiguration('world')],
            output='screen'),

        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            arguments=[
                '-entity', LaunchConfiguration('robotname'),  '-file', LaunchConfiguration('model')],
            output='screen'),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            arguments=[LaunchConfiguration('model')]
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='screen',
            arguments=['-d', LaunchConfiguration('rvizconfig')]
        )
    ])
